#include<ros/ros.h>
#include<geometry_msgs/Twist.h>
#include<iostream>
#include "std_msgs/Int8.h"
#include <std_msgs/Float64.h>


float WHEEL_L=2.8;
float WHEEL_R=0.6;
float WHEEL_PI=3.141693;
float v1=0,v2=0,v3=0,v4=0,v5=0,v6=0;
void turtle_cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{

  float speed_x,speed_y,speed_w;
  

  ROS_INFO("Received a /cmd_vel message!");
  speed_x = msg->angular.z;//v
  speed_y = 0;
  speed_w = msg->linear.x;//w
  

    v1 =speed_x+0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
   v3 =v1;
   v5 =v1;
   
   v2 =-(speed_x-0.5*WHEEL_L*speed_w);
   v4 =v2;
   v6 =v2;
  
  v1 =v1/(2.0*WHEEL_R*WHEEL_PI)*10;    //转换为轮子的速度　RPM
  v2 =v2/(2.0*WHEEL_R*WHEEL_PI)*10;
  v3 =v3/(2.0*WHEEL_R*WHEEL_PI)*10;
  v4 =v4/(2.0*WHEEL_R*WHEEL_PI)*10;
  v5 =v5/(2.0*WHEEL_R*WHEEL_PI)*10;
  v6 =v6/(2.0*WHEEL_R*WHEEL_PI)*10;
  
   //v1 =v1*WHEEL_RATIO*60;    //转每秒转换到RPM
  //v2 =v2*WHEEL_RATIO*60;
 // v3 =v3*WHEEL_RATIO*60;
 // v4 =v4*WHEEL_RATIO*60;
 // v3 =v3*WHEEL_RATIO*60;
 // v4 =v4*WHEEL_RATIO*60;
  
 

}


void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{

  float speed_x,speed_y,speed_w;
  

  ROS_INFO("Received a /cmd_vel message!");
  speed_x = msg->angular.z;//v
  speed_y = 0;
  speed_w = msg->linear.x;//w
  

    v1 =speed_x+0.5*WHEEL_L*speed_w;   //左边    //转化为每个轮子的线速度
   v3 =v1;
   v5 =v1;
   
   v2 =-(speed_x-0.5*WHEEL_L*speed_w);
   v4 =v2;
   v6 =v2;
  
  v1 =v1/(2.0*WHEEL_R*WHEEL_PI)*10;    //转换为轮子的速度　RPM
  v2 =v2/(2.0*WHEEL_R*WHEEL_PI)*10;
  v3 =v3/(2.0*WHEEL_R*WHEEL_PI)*10;
  v4 =v4/(2.0*WHEEL_R*WHEEL_PI)*10;
  v5 =v5/(2.0*WHEEL_R*WHEEL_PI)*10;
  v6 =v6/(2.0*WHEEL_R*WHEEL_PI)*10;
  
   //v1 =v1*WHEEL_RATIO*60;    //转每秒转换到RPM
  //v2 =v2*WHEEL_RATIO*60;
 // v3 =v3*WHEEL_RATIO*60;
 // v4 =v4*WHEEL_RATIO*60;
 // v3 =v3*WHEEL_RATIO*60;
 // v4 =v4*WHEEL_RATIO*60;
  
 

}
 
int main(int argc, char** argv)
{
	ros::init(argc, argv, "motive_control");
	ros::NodeHandle n;
	ros::Subscriber tsub = n.subscribe("/turtle1/cmd_vel", 1000, turtle_cmd_vel_callback);
        ros::Subscriber sub = n.subscribe("/cmd_vel", 1000, cmd_vel_callback);
        ros::Publisher left_pub = n.advertise<std_msgs::Int8>("MotionPubLeft", 1000);
	ros::Publisher right_pub = n.advertise<std_msgs::Int8>("MotionPubRight", 1000);
 ros::Rate loop_rate(10);

while (ros::ok())
  {
    
    std_msgs::Int8 msg1;
    std_msgs::Int8 msg2;
    msg1.data = v1;
    msg2.data = v2;
    ROS_INFO("%d", msg1.data); //ROS_INFO("%f", msg.data.c_str())
    ROS_INFO("%d", msg2.data);
    left_pub.publish(msg1);
    right_pub.publish(msg2);

    ros::spinOnce();
   
    loop_rate.sleep();
    

  }


  return 0;
}

